#include "polyhedron.h"
#include <GL/gl.h>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <cmath>
#include <cassert>

#include <CGAL/intersections.h>

/// A function object for computing plane equations
struct Plane_equation {
    template <class Facet>
    typename Facet::Plane_3 operator()( Facet& f) {
        typename Facet::Halfedge_handle h = f.halfedge();
        typedef typename Facet::Plane_3 Plane;
        return Plane ( h->vertex()->point(),
                       h->next()->vertex()->point(),
                       h->next()->next()->vertex()->point());
    }
};

/// A function object for computing bounding boxes
struct Bounding_box : public std::unary_function<Point, void>
{
    Bbox box;
    Bounding_box() : box(1e10,1e10,1e10,-1e10,-1e10,-1e10) {}
    void operator()(const Point& p) { 
        box = box + Bbox(p[0],p[1],p[2],p[0],p[1],p[2]);
    }
};


/// Computes face planes from 3 vertices
void Polyhedron::compute_facet_planes () {
    std::transform(facets_begin(), facets_end(), planes_begin(),
                   Plane_equation());
}

/// A function object for computing vertex normals 
struct Vertex_normal : public std::unary_function<Polyhedron::Vertex,void>
{
    void operator() (Polyhedron::Vertex& v) {
        Polyhedron::Vertex::Halfedge_around_vertex_const_circulator 
            hbegin = v.vertex_begin(), h = hbegin;
        int n = 0;
        Vector sum(0,0,0);
        do {
            if (h->facet() != 0) {
                Vector v (h->facet()->plane().orthogonal_vector());
                sum = sum + v;
                n++;
            }
        } while (++h != hbegin);
        assert (n > 0);
        v.normal = sum * (1.0/n);
    }
};


/// Computes a bounding box for the polyhedron
void Polyhedron::compute_box () {
    box = (std::for_each(points_begin(), points_end(), Bounding_box())).box;
}

/// Computes a bounding box for the polyhedron
void Polyhedron::compute_vertex_normals () {
    std::for_each(vertices_begin(), vertices_end(), Vertex_normal());
}

/// Computes an averaged normal for the vertex given by vtx_he->vertex() by considering
/// its use in displaying vtx_he->facet(). This means that the estimated vertex normal
/// should be an average of the normals of faces incident on the vertex, but only
/// those that make a relatively small angle with face vtx_he->facet(). The
/// minimumCos parameter is the minimum cosine admitted for such angles.
/// @param vtx_he half edge handle pointing to the vertex.
/// @param minimumCos minimum cosine admitted between the normal of vtx_he->facet() and
/// the normal of vertex-contiguous facet in order to include it in the normal average.
/// @return a smooth (average) normal vector at the vertex.
Vector smoothNormal (Halfedge_const_handle vtx_he, double minimumCos = 0.5) {
    
    Polyhedron::Vertex::Halfedge_around_vertex_const_circulator 
	hbegin = vtx_he->vertex()->vertex_begin(), h = hbegin;
    int n = 0;
    Vector mainNormal = normalize(vtx_he->facet()->plane().orthogonal_vector());
    Vector sum(0,0,0);
    do {
	if (h->facet() != 0) {
	    Vector v = normalize (h->facet()->plane().orthogonal_vector());
	    if (v*mainNormal >= minimumCos) {
		sum = sum + v;
		n++;
	    }
	}
    } while (++h != hbegin);
    assert (n > 0);
    if (n==0) return sum;
    return sum * (1.0/n);
}

/// Sets the vertex normal computation method
/// @param minCos minimum cosine admitted between the normal of edge adjoining facets and
/// in order to include it in the normal average. 1 means no vertex smoothing, while -1 means
/// all face normals are used to smooth the vertex normal. 
void Polyhedron::normal_policy (double minCos) 
{
    assert (minCos >= -1 && minCos <= 1);
    minimumCos = minCos;
}

/// Opengl drawing code.
/// @param minCos cosine of minimum angle used in vertex normal 
///  averaging. -1 means all face normals are used in the average, and +1
///  means only the original face normal is used (flat shading).
void Polyhedron::draw () const
{
    
    bool flat = minimumCos == 1;
    bool smooth = minimumCos == -1;
    for (Facet_const_iterator f = facets_begin(); f != facets_end(); ++f) {
        Facet::Halfedge_around_facet_const_circulator 
            hbegin = f->facet_begin(), h = hbegin;
        glBegin (GL_POLYGON);
        if (flat) {
            Vector fnormal = f->plane().orthogonal_vector();
            glNormal3f (fnormal[0],fnormal[1],fnormal[2]);
        }
        do {
            const Point& pt = h->vertex()->point();
            if (smooth) {
		Vector vnormal (h->vertex()->normal);
		glNormal3f (vnormal[0],vnormal[1],vnormal[2]);
            } else if (!flat) {
		Vector vnormal (smoothNormal(h, minimumCos));
		glNormal3f (vnormal[0],vnormal[1],vnormal[2]);
	    }		
            glVertex3f (pt[0], pt[1], pt[2]); 
        } while (++h != hbegin);
        glEnd ();
    }
    
}

/// Reads an off model into a polyhedron data structure and returns it
/// @param filename Name of the file containing model
/// @return pointer to newly read model or 0 if unsuccessful
///
Polyhedron* Polyhedron::load_off (const char * filename)
{
    std::fstream off_file (filename, std::ios_base::in);
    if (off_file) {
        Polyhedron * p = new Polyhedron ();
        off_file >> *p;
        if (off_file) {
	    p->compute_box ();
	    p->compute_facet_planes ();
	    p->compute_vertex_normals ();
            std::cout << "Polyhedral surface has "    
                      << p->size_of_vertices () << " vertices, "
                      << p->size_of_facets () << " facets, "
                      << "Bounding Box " << p->box << "\n";
            return p;
        }
    }      
    return 0; 
}


/// Returns a collection of intersection points between the polyhedron 
/// and the given ray. The points are stored in an arbitrary order into the vector
/// @param ray The intersected ray.
/// @return a vector of intersected points.
std::vector<Point> Polyhedron::intersect (const Ray& r) const
{
    std::vector<Point> result;
    
    // test all faces
    for (Facet_const_iterator f = facets_begin(); f != facets_end(); ++f) {
        Facet::Halfedge_around_facet_const_circulator 
            hbegin = f->facet_begin(), h = hbegin;
	const Point * v [3];
	int i = 0;
        do {
            v [i] = & (h->vertex()->point());
	    if (++i == 3) {
		// Three vertices make a triangle ...
		Triangle tri (*v[0], *v[1], *v[2]);
		if (do_intersect (r, tri)) {   
		    CGAL::Object obj = intersection(r, f->plane());
		    Point p;
		    if (CGAL::assign(p, obj)) {
			// Found a ray-face intersection
			result.push_back (p);
			break; // no need to test other triangles of the same face
		    }
		}
		v [1] = v [2];
		i = 2;
	    }
        } while (++h != hbegin);
    }

    return result;
}

